package edu.wpi.first.wpilibj.templates;

/**
 * @author Victoria
 */
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.*;

public class ArmClimber {

    private static final boolean DEBUGGING = true;

    private Victor leftMotor, rightMotor;
    
    //Hook speeds
    //Keep R and L speeds separate in case arms not running evenly and need to make values different
    private static final double SPEED = 1.0;
    private static final double EXTENDINGSPEED_R = SPEED;
    private static final double PULLINGSPEED_R = -SPEED;
    private static final double EXTENDINGSPEED_L = SPEED;
    private static final double PULLINGSPEED_L = -SPEED;
    private static final double SLOWSPEED = 0.25;
    
    //Limit switches and print locations on Driver Station
    private DigitalInput leftHookAtBottom, rightHookAtBottom, leftHookAtTop, rightHookAtTop;
    private DriverStationLCD.Line limitSwitchPrintLineTopLeft, limitSwitchPrintLineTopRight, limitSwitchPrintLineBottomLeft, limitSwitchPrintLineBottomRight;

    //Joystick and buttons
    Joystick joystick;
    private int rightUpButton, rightDownButton, leftUpButton, leftDownButton, bothUpButton, bothDownButton, bothUpSlow;    
    
    public ArmClimber(Joystick miscJoystick, int armMotorLeft, int armMotorRight, int limitSwitchPortTopR, int limitSwitchPortBottomR, int limitSwitchTopL, int limitSwitchBottomL, int rightUpButtonPort, int rightDownButtonPort, int leftUpButtonPort, int leftDownButtonPort, int bothUpButtonPort, int bothDownButtonPort, DriverStationLCD.Line limitSwtichLineTopLeft, DriverStationLCD.Line limitSwitchLineTopRight, DriverStationLCD.Line limitSwitchLineBottomLeft, DriverStationLCD.Line limitSwitchLineBottomRight, int slowButton) {
        
        //Arm Victors
        leftMotor = new Victor(armMotorLeft);
        rightMotor = new Victor(armMotorRight);
        
        //Joystick and buttons
        joystick = miscJoystick;
        rightUpButton = rightUpButtonPort;
        rightDownButton = rightDownButtonPort;
        leftUpButton = leftUpButtonPort;
        leftDownButton = leftDownButtonPort;
        bothUpButton = bothUpButtonPort;
        bothDownButton = bothDownButtonPort;
        bothUpSlow = slowButton;
        
        //Hook limit switches
        //Limit switch on the top is hit when the hook reaches the bottom and vice versa
        rightHookAtBottom = new DigitalInput(limitSwitchPortTopR);
        rightHookAtTop = new DigitalInput(limitSwitchPortBottomR);
        leftHookAtBottom = new DigitalInput(limitSwitchTopL);
        leftHookAtTop = new DigitalInput(limitSwitchBottomL);
        
        //Limit switch print lines on Driver Station
        limitSwitchPrintLineTopLeft = limitSwtichLineTopLeft;
        limitSwitchPrintLineTopRight = limitSwitchLineTopRight;
        limitSwitchPrintLineBottomLeft = limitSwitchLineBottomLeft;
        limitSwitchPrintLineBottomRight = limitSwitchLineBottomRight;
    }

    public void armDrive() {
        
        //First check if buttons to control both are being pressed - else use individual arm control
        if (joystick.getRawButton(bothUpButton)) { //&& !rightHookAtTop.get() && !leftHookAtTop.get()) { //If button to move both hooks up is pressed and bottom limit switches are not
            rightMotor.set(EXTENDINGSPEED_R);
            leftMotor.set(-EXTENDINGSPEED_L);
            
        } else if (joystick.getRawButton(bothDownButton)) {// && !rightHookAtBottom.get() && !leftHookAtBottom.get()) { //If button to move both hooks down is pressed and top limit switches are not
            rightMotor.set(PULLINGSPEED_R);
            leftMotor.set(-PULLINGSPEED_L);
            
        } else if (joystick.getRawButton(bothUpSlow)){
            rightMotor.set(SLOWSPEED);
            leftMotor.set(-SLOWSPEED);
        } else {
            if (joystick.getRawButton(rightUpButton)) {// && !rightHookAtTop.get()) { //If button to move right hook up is pressed and bottom right limit switch is not
                rightMotor.set(EXTENDINGSPEED_R);
            } else if (joystick.getRawButton(rightDownButton)) {// && !rightHookAtBottom.get()) { //If button to move right hook down is pressed and top right limit switch is not
                rightMotor.set(PULLINGSPEED_R);
            } else { //If no button is pressed to drive right hooks
                rightMotor.set(0);
            }

            if (joystick.getRawButton(leftUpButton)) {// && !leftHookAtTop.get()) { //If button to move left hook up is pressed and bottom left limit switch is not
                leftMotor.set(-EXTENDINGSPEED_L);
            } else if (joystick.getRawButton(leftDownButton)) {// && !leftHookAtBottom.get()) { //If button to move left hook down is pressed and top left limit switch is not
                leftMotor.set(-PULLINGSPEED_L);
            } else { //If not button is pressed to drive left hooks
                leftMotor.set(0);
            }
        }

        //Prints limit switch values to the driver station
//        DriverStationLCD.getInstance().println(limitSwitchPrintLineTopLeft, 1, "R at bottom: " + rightHookAtBottom.get());
//        DriverStationLCD.getInstance().println(limitSwitchPrintLineTopRight, 1, " L at bottom: " + leftHookAtBottom.get());
//        DriverStationLCD.getInstance().println(limitSwitchPrintLineBottomLeft, 1, "R at top: " + rightHookAtTop.get());
//        DriverStationLCD.getInstance().println(limitSwitchPrintLineBottomRight, 1, " L at top: " + leftHookAtTop.get());
        DriverStationLCD.getInstance().updateLCD();

        //Prints limit switch values to output window
        if (DEBUGGING) {
            //System.out.println("R at bottom: " + rightHookAtBottom.get() + " L at bottom: " + leftHookAtBottom.get() + " R at top: " + rightHookAtTop.get() + " L at top: " + leftHookAtTop.get());
            System.out.println("R up: " + joystick.getRawButton(rightUpButton) + " R down: " + rightDownButton + " L up: " + leftUpButton + " L down: " + leftDownButton + " R speed:" + rightMotor.get() + " L speed " + leftMotor.get());
        }
    }
}